
// Copyright (C) 2011 Luca Piccioni
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <exception>

#include "Derm.Simd.h"

using namespace std;

/** Translate a vector of 4 single-precision floating-point by a matrix 4x4.
  *
  * @note Additional arguments passed by registers
  *		- ESI: address for loading 3 single-precision floating-point
  *		- EDI: address for storing 3 single-precision floating-point
  *		- XMM0: column 0 of the matrix involved in multiplication
  *		- XMM1: column 1 of the matrix involved in multiplication
  *		- XMM2: column 2 of the matrix involved in multiplication
  *		- XMM3: column 3 of the matrix involved in multiplication
  */
__forceinline void Vector4_Add_Vector4_NoAlign_SSE()
{
	__asm {
		movups xmm0, [esi][ 0];
		movups xmm1, [esi][ 4];
		movups xmm2, [esi][ 8];
		movups xmm3, [esi][12];

		movups xmm4, [edi][ 0];
		movups xmm5, [edi][ 4];
		movups xmm6, [edi][ 8];
		movups xmm7, [edi][12];

		addps xmm0, xmm4;
		addps xmm1, xmm5;
		addps xmm2, xmm6;
		addps xmm3, xmm7;

		movups [edi][ 0], xmm0;
		movups [edi][ 4], xmm1;
		movups [edi][ 8], xmm2;
		movups [edi][12], xmm3;
	}
}

/** Translate a vector of 4 single-precision floating-point by a matrix 4x4.
  *
  * @note Additional arguments passed by registers
  *		- ESI: address for loading 3 single-precision floating-point
  *		- EDI: address for storing 3 single-precision floating-point
  *		- XMM0: column 0 of the matrix involved in multiplication
  *		- XMM1: column 1 of the matrix involved in multiplication
  *		- XMM2: column 2 of the matrix involved in multiplication
  *		- XMM3: column 3 of the matrix involved in multiplication
  */
__forceinline void Vector4_Add_Vector4_Align_SSE()
{
	__asm {
		movaps xmm0, [esi][ 0];
		movaps xmm1, [esi][ 4];
		movaps xmm2, [esi][ 8];
		movaps xmm3, [esi][12];

		movups xmm4, [edi][ 0];
		movups xmm5, [edi][ 4];
		movups xmm6, [edi][ 8];
		movups xmm7, [edi][12];

		addps xmm0, xmm4;
		addps xmm1, xmm5;
		addps xmm2, xmm6;
		addps xmm3, xmm7;

		movaps [edi][ 0], xmm0;
		movaps [edi][ 4], xmm1;
		movaps [edi][ 8], xmm2;
		movaps [edi][12], xmm3;
	}
}

/** Multiply a vector of 3 single-precision floating-point by a matrix 4x4.
  *
  * @note Additional arguments passed by registers
  *		- ESI: address for loading 3 single-precision floating-point
  *		- EDI: address for storing 3 single-precision floating-point
  *		- XMM0: column 0 of the matrix involved in multiplication
  *		- XMM1: column 1 of the matrix involved in multiplication
  *		- XMM2: column 2 of the matrix involved in multiplication
  *		- XMM3: column 3 of the matrix involved in multiplication
  */
__forceinline void Vector3_Multiply_Matrix4x4_SSE()
{
	// --- Overview scheme ------------------------------------------------
	// | v00 |   | m00 m10 m20 m30 |   | r00 |
	// | v01 | * | m01 m11 m21 m31 | = | r01 | 
	// | v02 |   | m02 m12 m22 m32 |   | r02 |
	// | 1.0 |   | m03 m13 m23 m33 |   | xxx |
	// --------------------------------------------------------------------

	// xmm0 =      xmm0     +      xmm5     +     xmm6      +     xmm7
	//         xmm4 * xmm0  +  xmm5 * xmm1  +  xmm6 * xmm2  +  xmm7 * xmm3
	// r00  =  v00  *  m00  +  v01  *  m01  +  v02  *  m02  +  1.0  *  m03
	// r01  =  v00  *  m10  +  v01  *  m11  +  v02  *  m12  +  1.0  *  m13
	// r02  =  v00  *  m20  +  v01  *  m21  +  v02  *  m22  +  1.0  *  m23

	__asm {
		// Load vector
		movss xmm4, [esi][0];
		movss xmm5, [esi][4];
		movss xmm6, [esi][8];

		shufps xmm4, xmm4, 0x00;	/* 00000000b -> 0x00 (1th component on all slots) */
		shufps xmm5, xmm5, 0x00;	/* 00000000b -> 0x00 (1th component on all slots) */
		shufps xmm6, xmm6, 0x00;	/* 00000000b -> 0x00 (1th component on all slots) */

		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		//movups [edi][0], xmm0; ???
	}
}

/** Multiply a vector of 4 single-precision floating-point by a matrix 4x4.
  *
  * @note Additional arguments passed by registers
  *		- ESI: address for loading 3 single-precision floating-point
  *		- EDI: address for storing 3 single-precision floating-point
  *		- XMM0: column 0 of the matrix involved in multiplication
  *		- XMM1: column 1 of the matrix involved in multiplication
  *		- XMM2: column 2 of the matrix involved in multiplication
  *		- XMM3: column 3 of the matrix involved in multiplication
  */
__forceinline void Vector4_Multiply_Matrix4x4_SSE()
{
	// --- Overview scheme ------------------------------------------------
	// | v00 |   | m00 m10 m20 m30 |   | r00 |
	// | v01 | * | m01 m11 m21 m31 | = | r01 | 
	// | v02 |   | m02 m12 m22 m32 |   | r02 |
	// | v03 |   | m03 m13 m23 m33 |   | r03 |
	// --------------------------------------------------------------------

	// xmm0 =      xmm0     +      xmm5     +     xmm6      +     xmm7
	//         xmm4 * xmm0  +  xmm5 * xmm1  +  xmm6 * xmm2  +  xmm7 * xmm3
	// r00  =  v00  *  m00  +  v01  *  m01  +  v02  *  m02  +  v03  *  m03
	// r01  =  v00  *  m10  +  v01  *  m11  +  v02  *  m12  +  v03  *  m13
	// r02  =  v00  *  m20  +  v01  *  m21  +  v02  *  m22  +  v03  *  m23
	// r03  =  v00  *  m30  +  v01  *  m31  +  v02  *  m32  +  v03  *  m33

	__asm {
		// Load vector
		movups xmm4, [esi][ 0];
		movups xmm5, xmm4;
		movups xmm6, xmm4;
		movups xmm7, xmm4;

		shufps xmm4, xmm4, 0x00;	/* 00000000b -> 0x00 (1th component on all slots) */
		shufps xmm5, xmm5, 0x55;	/* 01010101b -> 0x55 (2th component on all slots) */
		shufps xmm6, xmm6, 0xAA;	/* 10101010b -> 0xAA (3th component on all slots) */
		shufps xmm7, xmm7, 0xFF;	/* 11111111b -> 0xFF (4th component on all slots) */

		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;
		mulps xmm3, xmm7;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		movups [edi][0], xmm0;
	}
}

/** Multiply a vector of 3 single-precision floating-point by a matrix 4x4.
  *
  * @param dstVector		Vector storing the computed vector.
  * @param srcVector		Vector used as input (left multiplication operand).
  * @param matrix			Matrix used as input (right multiplication operand).
  */
void Vector3_Multiply_Matrix4x4_SSE(float *dstVector, float *srcVector, float *matrix)
{
	__asm {
		mov esi, matrix;

		// Load matrix
		movups xmm0, [esi][ 0];
		movups xmm1, [esi][16];
		movups xmm2, [esi][32];
		movups xmm3, [esi][48];

		mov esi, srcVector;
		mov edi, dstVector;
	}

	Vector3_Multiply_Matrix4x4_SSE();
}

/** Multiply a sequence of vectors of 3 single-precision floating-point by a matrix 4x4.
  *
  * @param dstVector		Vector storing the computed vector.
  * @param srcVector		Vector used as input (left multiplication operand).
  * @param matrix			Matrix used as input (right multiplication operand).
  * @param count			The number of vectors to compute.
  */
void Vector3_Multiply_Matrix4x4_Batch_SSE(float *dstVector, float *srcVector, float *matrix, unsigned int count)
{
	__asm {
		mov esi, matrix;

		// Load matrix
		movups xmm0, [esi][ 0];
		movups xmm1, [esi][16];
		movups xmm2, [esi][32];
		movups xmm3, [esi][48];
	}

	for (unsigned int i = 0; i < count; i++) {
		const float *srcAddress = srcVector + i * 3;
		const float *dstAddress = dstVector + i * 3;

		__asm {
			mov esi, srcAddress;
			mov edi, dstAddress;
		}
		
	}
}

/** Multiply a vector of 4 single-precision floating-point by a matrix 4x4.
  *
  * @param dstVector		Vector storing the computed vector.
  * @param srcVector		Vector used as input (left multiplication operand).
  * @param matrix			Matrix used as input (right multiplication operand).
  */
void Vector4_Multiply_Matrix4x4_SSE(float *dstVector, float *srcVector, float *matrix)
{
	__asm {
		mov esi, matrix;

		// Load matrix
		movups xmm0, [esi][ 0];
		movups xmm1, [esi][16];
		movups xmm2, [esi][32];
		movups xmm3, [esi][48];

		mov esi, srcVector;
		mov edi, dstVector;
	}
	
	
}
/** Multiply a sequence of vectors of 4 single-precision floating-point by a matrix 4x4.
  *
  * @param dstVector		Vector storing the computed vector.
  * @param srcVector		Vector used as input (left multiplication operand).
  * @param matrix			Matrix used as input (right multiplication operand).
  * @param count			The number of vectors to compute.
  */
void Vector4_MultiplyMatrix_Batch_SSE(float *dstVector, float *srcVector, float *matrix, unsigned int count)
{
	__asm {
		mov esi, matrix;

		// Load matrix
		movups xmm0, [esi][ 0];
		movups xmm1, [esi][16];
		movups xmm2, [esi][32];
		movups xmm3, [esi][48];
	}

	for (unsigned int i = 0; i < count; i++) {
		const float *srcAddress = srcVector + i * 4;
		const float *dstAddress = dstVector + i * 4;

		__asm {
			mov esi, srcAddress;
			mov edi, dstAddress;
		}
		
	}
}

//=============================================================================
// Matrix4x4_Multiply_Matrix4x4
//=============================================================================

void DERM_SIMD_API Matrix4x4_Multiply_Matrix4x4(float *result, float *left, float *right)
{
	result[0] = left[0] * right[0] + left[4] * right[1] + left[8] * right[2] + left[12] * right[3];
	result[1] = left[1] * right[0] + left[5] * right[1] + left[9] * right[2] + left[13] * right[3];
	result[2] = left[2] * right[0] + left[6] * right[1] + left[10] * right[2] + left[14] * right[3];
	result[3] = left[3] * right[0] + left[7] * right[1] + left[11] * right[2] + left[15] * right[3];

	result[4] = left[0] * right[4] + left[4] * right[5] + left[8] * right[6] + left[12] * right[7];
	result[5] = left[1] * right[4] + left[5] * right[5] + left[9] * right[6] + left[13] * right[7];
	result[6] = left[2] * right[4] + left[6] * right[5] + left[10] * right[6] + left[14] * right[7];
	result[7] = left[3] * right[4] + left[7] * right[5] + left[11] * right[6] + left[15] * right[7];

	result[8] = left[0] * right[8] + left[4] * right[9] + left[8] * right[10] + left[12] * right[11];
	result[9] = left[1] * right[8] + left[5] * right[9] + left[9] * right[10] + left[13] * right[11];
	result[10] = left[2] * right[8] + left[6] * right[9] + left[10] * right[10] + left[14] * right[11];
	result[11] = left[3] * right[8] + left[7] * right[9] + left[11] * right[10] + left[15] * right[11];

	result[12] = left[0] * right[12] + left[4] * right[13] + left[8] * right[14] + left[12] * right[15];
	result[13] = left[1] * right[12] + left[5] * right[13] + left[9] * right[14] + left[13] * right[15];
	result[14] = left[2] * right[12] + left[6] * right[13] + left[10] * right[14] + left[14] * right[15];
	result[15] = left[3] * right[12] + left[7] * right[13] + left[11] * right[14] + left[15] * right[15];
}

void DERM_SIMD_API Matrix4x4_Multiply_Matrix4x4_SSE(float *result, float *left, float *right)
{
	// --- Overview scheme ------------------------------------------------
	// | l00 l10 l20 l30 |   | r00 r10 m20 m30 |   | m00 m10 m20 m30 |
	// | l01 l11 l21 l31 | * | r01 r11 m21 m31 | = | m01 m11 m21 m31 | 
	// | l02 l12 l22 l32 |   | r02 r12 m22 m32 |   | m02 m12 m22 m32 |
	// | l03 l13 l23 l33 |   | r03 r13 m23 m33 |   | m03 m13 m23 m33 |
	// --------------------------------------------------------------------

	//             xmm0     +      xmm1     +     xmm2      +     xmm3
	//         xmm4 * xmm0  +  xmm5 * xmm1  +  xmm6 * xmm2  +  xmm7 * xmm3
	// r00 =    l00 * r00   +   l10 * r01   +   l20 * r02   +   l30 * r03
	// r01 =    l01 * r00   +   l11 * r01   +   l21 * r02   +   l31 * r03
	// r02 =    l02 * r00   +   l12 * r01   +   l22 * r02   +   l32 * r03
	// r03 =    l03 * r00   +   l13 * r01   +   l23 * r02   +   l33 * r03

	// r10 =    l00 * r10   +   l10 * r11   +   l20 * r12   +   l30 * r13
	// r11 =    l01 * r10   +   l11 * r11   +   l21 * r12   +   l31 * r13
	// r12 =    l02 * r10   +   l12 * r11   +   l22 * r12   +   l32 * r13
	// r13 =    l03 * r10   +   l13 * r11   +   l23 * r12   +   l33 * r13

	__asm {
		mov esi, right;
		mov edi, left;

		// Load left colums
		movups xmm4, [edi][ 0];
		movups xmm5, [edi][16];
		movups xmm6, [edi][32];
		movups xmm7, [edi][48];

		// From now EDI points to result matrix
		mov edi, result;

		// Column 0
		movss xmm0, [esi][ 0];
		movss xmm1, [esi][ 4];
		movss xmm2, [esi][ 8];
		movss xmm3, [esi][12];

		shufps xmm0, xmm0, 0x00;
		shufps xmm1, xmm1, 0x00;
		shufps xmm2, xmm2, 0x00;
		shufps xmm3, xmm3, 0x00;
		
		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;
		mulps xmm3, xmm7;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		movdqu [edi][ 0], xmm0;

		// Column 1
		movss xmm0, [esi][16];
		movss xmm1, [esi][20];
		movss xmm2, [esi][24];
		movss xmm3, [esi][28];

		shufps xmm0, xmm0, 0x00;
		shufps xmm1, xmm1, 0x00;
		shufps xmm2, xmm2, 0x00;
		shufps xmm3, xmm3, 0x00;
		
		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;
		mulps xmm3, xmm7;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		movdqu [edi][16], xmm0;

		// Column 2
		movss xmm0, [esi][32];
		movss xmm1, [esi][36];
		movss xmm2, [esi][40];
		movss xmm3, [esi][44];

		shufps xmm0, xmm0, 0x00;
		shufps xmm1, xmm1, 0x00;
		shufps xmm2, xmm2, 0x00;
		shufps xmm3, xmm3, 0x00;
		
		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;
		mulps xmm3, xmm7;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		movdqu [edi][32], xmm0;

		// Column 3
		movss xmm0, [esi][48];
		movss xmm1, [esi][52];
		movss xmm2, [esi][56];
		movss xmm3, [esi][60];

		shufps xmm0, xmm0, 0x00;
		shufps xmm1, xmm1, 0x00;
		shufps xmm2, xmm2, 0x00;
		shufps xmm3, xmm3, 0x00;
		
		mulps xmm0, xmm4;
		mulps xmm1, xmm5;
		mulps xmm2, xmm6;
		mulps xmm3, xmm7;

		addps xmm0, xmm1;
		addps xmm2, xmm3;
		addps xmm0, xmm2;

		movdqu [edi][48], xmm0
	}
}

//=============================================================================
// Matrix4x4_Concatenate
//=============================================================================

void DERM_SIMD_API Matrix4x4_Concatenate(float *matrices[], unsigned int count)
{
	for (int i = 1; i < count; i++)
		Matrix4x4_Multiply_Matrix4x4(matrices[0], matrices[i], matrices[0]);
}

void DERM_SIMD_API Matrix4x4_Concatenate_SSE(float *matrices[], unsigned int count)
{
	for (int i = 1; i < count; i++)
		Matrix4x4_Multiply_Matrix4x4_SSE(matrices[0], matrices[i], matrices[0]);
}